#include "MiniParaCurve.h"

using namespace XYF;

#define ITERATION 10

MiniParaCurve::MiniParaCurve(Func13 f, Real left, Real right) : m_f(f), m_left(left), m_right(right)
{
}

int XYF::MiniParaCurve::GetType()
{
    return MiniObject::para_curve;
}

Real MiniParaCurve::GetBegin() const
{
    return m_left;
}

Real MiniParaCurve::GetEnd() const
{
    return m_right;
}

Vector3f MiniParaCurve::CurvePoint(Real u) const
{
    return m_f(u);
}

Vector3f MiniParaCurve::CurveDerivPoint(Real u) const
{
    Real h = 1e-8;
    return (m_f(u + h) - m_f(u - h)) / (2 * h);
}

Vector3f MiniParaCurve::CurveDeriv2Point(Real u) const
{
    Real h = 1e-4;
    return (m_f(u + h) - m_f(u) * 2 + m_f(u - h)) / (h * h);
}

MiniArray<Vector> MiniParaCurve::CurveOthroProjection(MiniBSplineSurface *surface, Real t0, Real t1, int N, Real tol) const
{
    // 构造求解的函数
    FuncXXt F = [=](Vector x, Real t) -> Vector
    {
        Real h = 1e-8;
        Vector3f dp = (m_f(t + h) - m_f(t - h)) / (2 * h);
        MiniArray2D<Vector3f> g = surface->SurfaceDerivPoint(2, x[0], x[1]);
        Vector y = {dot(dp, g[1][0]), dot(dp, g[0][1])};

        // 空间点到投影点的向量，获得投影张量矩阵
        Vector3f pq = g[0][0] - m_f(t);
        Matrix K(2, 2);
        K(0, 0) = dot(g[1][0], g[1][0]) + dot(pq, g[2][0]);
        K(0, 1) = dot(g[1][0], g[0][1]) + dot(pq, g[1][1]);
        K(1, 0) = dot(g[0][1], g[1][0]) + dot(pq, g[1][1]);
        K(1, 1) = dot(g[0][1], g[0][1]) + dot(pq, g[0][2]);

        // 获得微分方程右边的向量值
        return K.inverse() * y;
    };

    // 首先计算一个单点的正交投影作为初始值，由于正交投影的稳定性，直接从曲面中心开始迭代投影
    Vector3f P0 = m_f(t0);
    Vector uv0 = surface->OrthoPointProjection(P0, {0.5, 0.5}, tol);

    // 终止条件以及步长
    Real k = (t1 - t0) / N;

    // 使用经典 RK 方法
    ODESolver solver;
    ClassicalRK method(k);

    // 注意要多给一个
    TimeGrid G(N + 1, Vector(2));
    G[0] = {uv0};
    solver.solve(F, G, &method);

    return G;
}

MiniArray<Vector> MiniParaCurve::CurveMarchProjection(MiniBSplineSurface *surface, Real t0, Real t1, int N, Real tol) const
{
    // 首先计算一个单点的正交投影作为初始值，由于正交投影的稳定性，直接从曲面中心开始迭代投影
    Vector3f P0 = m_f(t0);
    Vector uv = surface->OrthoPointProjection(P0, {0.5, 0.5}, tol);

    // 终止条件以及步长
    Real k = (t1 - t0) / N;
    Real t = t0;

    // 记录每一步的参数值
    MiniArray<Vector> UV(N + 1, Vector(2));
    UV[0] = uv;

    // 迭代步进
    for (int i = 0; i < N; i++)
    {
        // 获得曲面局部信息
        MiniArray2D<Vector3f> S = surface->SurfaceDerivPoint(3, uv[0], uv[1]);
        Vector3f Np = cross(S[1][0], S[0][1]);
        Vector3f N = m_f(t) - S[0][0];

        // 构造系数矩阵 G
        Matrix G(2, 2);
        G(0, 0) = dot(S[1][0], S[1][0]) + dot(N, S[2][0]);
        G(0, 1) = G(1, 0) = dot(S[1][0], S[0][1]) + dot(N, S[1][1]);
        G(1, 1) = dot(S[0][1], S[0][1]) + dot(N, S[0][2]);

        // 进行修正：N = q - p，比较 N 与 Np
        int fix = 0;
        Real epsilon = length(cross(N, Np)) / (length(N) * length(Np));
        while (epsilon > tol && fix < ITERATION)
        {
            // 求解修正方程，同时替换参数
            Vector Nuv = {dot(N, S[1][0]), dot(N, S[0][1])};
            Vector duv = G.inverse() * Nuv;
            uv = uv + duv;
            UV[i] = uv;

            // 重新计算
            S = surface->SurfaceDerivPoint(3, uv[0], uv[1]);
            Np = cross(S[1][0], S[0][1]);
            N = m_f(t) - S[0][0];

            G(0, 0) = dot(S[1][0], S[1][0]) + dot(N, S[2][0]);
            G(0, 1) = G(1, 0) = dot(S[1][0], S[0][1]) + dot(N, S[1][1]);
            G(1, 1) = dot(S[0][1], S[0][1]) + dot(N, S[0][2]);

            epsilon = length(cross(N, Np)) / (length(N) * length(Np));
            fix++;
        }

        // 求解一阶偏导
        Vector3f qk = CurveDerivPoint(t);
        Vector b1 = {dot(qk, S[1][0]), dot(qk, S[0][1])};
        Vector uv1 = G.inverse() * b1;

        // 求解二阶偏导
        Matrix A1(2, 2);
        A1(0, 0) = 3 * dot(S[2][0], S[1][0]) + dot(S[3][0], N);
        A1(0, 1) = A1(1, 0) = 2 * dot(S[1][1], S[1][0]) + dot(S[2][0], S[0][1]) + dot(S[2][1], N);
        A1(1, 1) = dot(S[0][2], S[1][0]) + 2 * dot(S[1][1], S[0][1]) + dot(S[1][2], N);

        Matrix A2(2, 2);
        A2(0, 0) = dot(S[2][0], S[0][1]) + 2 * dot(S[1][1], S[1][0]) + dot(S[2][1], N);
        A2(0, 1) = A2(1, 0) = 2 * dot(S[1][1], S[0][1]) + dot(S[0][2], S[1][0]) + dot(S[1][2], N);
        A2(1, 1) = 3 * dot(S[0][2], S[0][1]) + dot(S[0][3], N);

        Vector b2(2);
        Vector3f qkk = CurveDeriv2Point(t);

        b2[0] = dot(qkk, S[1][0]);
        b2[0] += 2 * dot(qk, S[2][0] * uv1[0] + S[1][1] * uv1[1]);

        // 减去二阶项
        for (int i = 0; i < 2; i++)
            for (int j = 0; j < 2; j++)
                b2[0] -= A1(i, j) * uv1[i] * uv1[j];

        b2[1] = dot(qkk, S[0][1]);
        b2[1] += 2 * dot(qk, S[1][1] * uv1[0] + S[0][2] * uv1[1]);

        // 减去二阶项
        for (int i = 0; i < 2; i++)
            for (int j = 0; j < 2; j++)
                b2[1] -= A2(i, j) * uv1[i] * uv1[j];

        // 解得 2 阶偏导
        Vector uv2 = G.inverse() * b2;

        // 利用 2 阶泰勒展开推进
        uv[0] += uv1[0] * k + 0.5 * uv2[0] * k * k;
        uv[1] += uv1[1] * k + 0.5 * uv2[1] * k * k;

        // 记录这一步参数
        UV[i + 1] = uv;
        t += k;
    }
    return UV;
}
